#!/usr/bin/python3
# -*- coding: utf-8 -*-

import sys,os,time
import rospy,cv2,cv_bridge

sys.path.append(os.path.join(os.path.dirname(os.path.abspath(__file__)),'sub'))
from sub.motion_control import motion_control
from sub.castle_function import castle_function
from sub.castlex_iot_mqtt import castlex_iot_mqtt
from playsound import playsound
from task_gui_control import *
rospy.init_node('main', log_level=rospy.INFO)





# ************motion_control*************
# mc = motion_control()
# arrival_bool = mc.Target_point([-0.6451631132145647, 0.05170592510949631, 0.0, 0.0, 0.0, -0.1299196991516246, 0.9915245189970601])
# arrival_list = mc.get_position() 
# print(arrival_list)



# ************iot_mqtt********************
# qq =  castlex_iot_mqtt()
# zw=qq.iot_mqtt_connect("HG_YINGYONG_KZ_0001", "192.168.3.121", 1883)
# print(zw)
# # # # 灯：turn     门铃：door   窗帘：curtain     闸机：Gate
# qq.iot_mqtt_send({"curtain":"true"})
# time.sleep(5)
# qq.iot_mqtt_send({"curtain":"false"})
# time.sleep(8)
# qq.iot_mqtt_send({"Gate":"true"})
# time.sleep(5)
# qq.iot_mqtt_send({"Gate":"false"})



# ************castle_function*************
# cf=castle_function()
# cf.ultraviolet_disinfection(1)
# time.sleep(45)
# cf.ultraviolet_disinfection(0)
#cf.spray_kill(1)
#time.sleep(5)
#cf.spray_kill(0)
# cf.cargo_control(1)
# time.sleep(2)
# cf.cargo_control(0)
# print('detect face id')
# id = False
# while id is False:
#     id = cf.cargo_face_detect()
#     print('id:', id)
# idcard = False
# print('detect id card')
# while idcard is False:
#     idcard = cf.cargo_idcard_detect()
#     print('idcard:', idcard)

# ********************taskGUI*********************** 
# task_gui = taskGUIManager()
# task_gui.start()
# task_gui. indicator_buttons(0)
# flag = task_gui. control_buttons()
# flag = task_gui. cam_detect (2)
# task_gui.Get_Recognition_rate()
# task_gui.indicator_string("high-genius")
# task_gui.close()

def get_iot_context():
    iot =  castlex_iot_mqtt()
    zw = iot.iot_mqtt_connect("HG_YINGYONG_KZ_0001", "192.168.3.121", 1883)
    print('iot:', zw)
    return iot

def get_location():
    print(' getting location')
    mc = motion_control()
    while True:
        print(' getting location')
        time.sleep(2)
        arrival_list = mc.get_position()
        print(arrival_list)

def control_by_voice():
    cf = castle_function()
    while not rospy.is_shutdown():
        id = cf.voice_off_line_task()
        time.sleep(0.5)
        if id == '00':
            mc = motion_control()
            mc.Target_point([0.8986702952735307, 0.12306403267365863, 0.0, 0.0, 0.0, -0.006819896608382211, 0.9999767442347102])
            playsound("/home/castlex/castlex_ws/src/castlex_voice_system/tools/voice_qiantai") 

def main_loop_spray_kill():
    cf = castle_function()
    iot = get_iot_context()
    points = [
        ([0.8986702952735307, 0.12306403267365863, 0.0, 0.0, 0.0, -0.006819896608382211, 0.9999767442347102], 
        'Gate'),
        ([1.108692256920416, 0.06610334215030889, 0.0, 0.0, 0.0, 0.13481643584332026, 0.9908705912612927], 
        'turn2'),
        ([1.4360196592792664, 0.13396564170649042, 0.0, 0.0, 0.0, 0.1338273722686743, 0.9910046591372119], 
        'door2'),
        ([1.535803183950172, -0.48013393048847747, 0.0, 0.0, 0.0, -0.2482309824617996, 0.9687008719651541],
        'curtain'),
        ([1.687134929294016, -0.837383388260349, 0.0, 0.0, 0.0, -0.2572945204485442, 0.966333032524064], 
        'turn3'),
        ([1.5184041038459348, -0.934008059399541, 0.0, 0.0, 0.0, -0.32916509193126775, 0.9442723877430071], 
        'turn1'),
        ([1.2112277066471284, -0.7268305822048933, 0.0, 0.0, 0.0, -0.2956241532068157, 0.9553043284947228], 
        'door1')
    ]
    mc = motion_control()
    cur_position = mc.get_position()
    # 开紫外消毒
    # cf.ultraviolet_disinfection(1)
    # time.sleep(37)
    # 开喷雾消毒
    cf.spray_kill(1)
    for pt in points:
        if pt[0]:
            mc.Target_point(pt[0])
            time.sleep(1)
            iot.iot_mqtt_send({pt[1]: 'true'})
            time.sleep(2)
            iot.iot_mqtt_send({pt[1]: 'false'})
    
    mc.Target_point(cur_position)
    # 关紫外消毒
    # cf.ultraviolet_disinfection(0)
    # time.sleep(30)
    # 关喷雾消毒
    cf.spray_kill(0)
    print('finished')

def main_loop_cargo():
    cf = castle_function()
    mc = motion_control()
    iot = get_iot_context()
    cur_position = mc.get_position()
    mc.Target_point([-0.6451631132145647, 0.05170592510949631, 0.0, 0.0, 0.0, -0.1299196991516246, 0.9915245189970601])
    time.sleep(1)
    iot.iot_mqtt_send({'turn2': 'true'})
    # 人脸识别
    id = False
    while id is False:
        id = cf.cargo_face_detect()
    if id is not False:
        cf.cargo_control(1)
        time.sleep(5)
        cf.cargo_control(0)
    # IC卡识别
    id = False
    while id is False:
        id = cf.cargo_idcard_detect()
    if id is not False:
        cf.cargo_control(1)
        time.sleep(5)
        cf.cargo_control(0)
    time.sleep(1)
    iot.iot_mqtt_send({'turn2': 'false'})
    mc.Target_point(cur_position)

if __name__ == '__main__':
    # get_location()
    # control_by_voice()
    # main_loop_spray_kill()
    main_loop_cargo()

